#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp>
#include <geometry_msgs/msg/pose.hpp>

class MapPublisher : public rclcpp::Node
{
public:
    MapPublisher()
        : Node("map_publisher")
    {
        publisher_ = this->create_publisher<nav_msgs::msg::OccupancyGrid>("map", 10);
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(500), std::bind(&MapPublisher::timer_callback, this));

        map_msg_.header.frame_id = "map";
        map_msg_.info.resolution = 1.0; // 1 meter per pixel
        map_msg_.info.width = MAP_WIDTH;
        map_msg_.info.height = MAP_HEIGHT;
        map_msg_.info.origin.position.x = 0.0;
        map_msg_.info.origin.position.y = 0.0;
        map_msg_.info.origin.position.z = 0.0;
        map_msg_.info.origin.orientation.w = 1.0;

        map_msg_.data.resize(MAP_WIDTH * MAP_HEIGHT);
        for (int y = 0; y < MAP_HEIGHT; ++y)
        {
            for (int x = 0; x < MAP_WIDTH; ++x)
            {
                map_msg_.data[y * MAP_WIDTH + x] = map[y][x] * 100; // 0 or 100
            }
        }
    }

private:
    void timer_callback()
    {
        map_msg_.header.stamp = this->get_clock()->now();
        publisher_->publish(map_msg_);
    }

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr publisher_;
    nav_msgs::msg::OccupancyGrid map_msg_;

    // 定义地图大小
    static constexpr int MAP_WIDTH = 10;
    static constexpr int MAP_HEIGHT = 10;

    // 定义地图，1表示障碍物，0表示可通过
    static constexpr int map[MAP_HEIGHT][MAP_WIDTH] = {
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 1, 1, 1, 1, 1, 1, 1, 0},
        {0, 1, 0, 0, 0, 0, 0, 0, 0, 0},
        {0, 1, 0, 1, 1, 1, 1, 1, 1, 0},
        {0, 1, 0, 1, 0, 0, 0, 0, 0, 0},
        {0, 1, 0, 1, 0, 1, 1, 1, 1, 0},
        {0, 1, 0, 1, 0, 1, 0, 0, 0, 0},
        {0, 1, 0, 1, 0, 1, 0, 1, 1, 0},
        {0, 1, 0, 0, 0, 0, 0, 1, 0, 0},
        {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}};
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MapPublisher>());
    rclcpp::shutdown();
    return 0;
}